#ifndef WORLDMODEL_H_
#define WORLDMODEL_H_

#include <boost/property_tree/ptree.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <cmath>
#include <vector>
#include <map>

using boost::property_tree::ptree;

#define FORWARDANGLE 0
#define RIGHTANGLE -M_PI/2

class Message {
  public:

    Message(std::string address, std::string data)
    {
        this->address = address;
        this->data = data;
    }

    std::string getSender() const {return address;}
    const void* getData() const {return &data[0];}
    std::string getDataString() const { return data; }
    size_t getDataLength() const {return data.size();}

  private:
    std::string address;
    std::string data;
};

class IRMessage {
  public:

    IRMessage(int address, std::string data, double intens):
      address(address),
      data(data),
      intensity(intens)
    {}

    int getSender() const {return address;}
    const void* getData() const {return &data[0];}
    size_t getDataLength() const {return data.size();}
    double getIntensity() {return intensity;}

  private:
    int address;
    std::string data;
    double intensity;
};

struct WorldModel
{
	std::string robotId;
	std::string robotModel; // KIT or AW
	int robotNumber;
	double time;

	double irSensor[8];	// Distance sensor inputs
	bool connected[4]; // which face is connected
	bool toConnect[4]; // which face needs to be connected
	bool toDisconnect[4]; // which face needs to be disconnected
	bool presence[4];

	double xPos;			/// < GPS x coordinate
	double yPos;			/// < GPS y coordinate
	double zPos;			/// < GPS z coordinate

	double xRot;
	double yRot;
	double zRot;

	double desiredHingePosition;
	double desiredSpeed[2];

	double currentHingePosition;
	
	bool isEgg;
	int organismSize;
	
	bool changePosAsked;
	std::vector<double> desiredTranslation;

	bool changeRotAsked;
	std::vector<double> desiredRotation;

	boost::ptr_vector<Message> msgToSend;
	boost::ptr_vector<Message> msgReceived;

	boost::ptr_vector<IRMessage> irMsgToSend;
	boost::ptr_vector<IRMessage> irMsgReceived;

	boost::ptr_vector<Message> radioMsgToSend;
	boost::ptr_vector<Message> radioMsgReceived;
    
    std::map<int,int> ledColours;

	ptree additionalProperties;

	int organismChannel; // the channel on which the organism emitter/receiver pair should work

};
#endif
